#include <asm/regdef.h>
//#include <asm/cp0regdef.h>
#include <asm/asm.h>
#include <stackframe.h>
#include <mmu.h>

.macro	__build_clear_sti
	STI
.endm

.macro	__build_clear_cli
	CLI
.endm

.macro	BUILD_HANDLER exception handler clear
	.align	5
	NESTED(handle_\exception, TF_SIZE, sp)
	csrrw	zero, sscratch, sp
	la	sp, KERNEL_SP
	ld	sp, 0(sp)
	SAVE_ALL
	move	a0, sp
	call	\handler
	j	ret_from_exception
	/*.set	noat

nop

	SAVE_ALL				
	__build_clear_\clear
	.set	at
	move	a0, sp
	jal	\handler
	nop
	j	ret_from_exception
	nop*/
	END(handle_\exception)
.endm

FEXPORT(ret_from_exception)
	/*.set noat
	.set noreorder
	RESTORE_SOME
	.set at
	lw	k0,TF_EPC(sp)				 
	lw	sp,TF_REG29(sp)
//1:	j	1b
	nop
	jr	k0								 
	rfe	*/							 
	RESTORE_ALL
	sret


.align	5
NESTED(handle_int, TF_SIZE, sp)
	csrrw	zero, sscratch, sp
	la	sp, TIMESTACK
	SAVE_ALL
	CLI
	call	clk_set_next_event
	call	sched_yield
END(handle_int)

	.extern delay

LEAF(do_reserved)
END(do_reserved)

/* Requires:
 * a0: Cause
 * a1: Bad Vaddr(stval)
 * a2: EPC, not necessary here.
 * a3: satp
 */
NESTED(exc_hand_pgfault, 0, sp)
	li	t1, 0x0FFFFFFFFFFF
	and	t0, t1, a3
	sll	t0, t0, 12	// t0 is paddr of vpt2.
	move	t4, t0		// t4 to save vpt2 for further use
	srl	t1, a1, 30	// a1 is badvaddr
	andi	t1, t1, 0x1ff	// t1 is VPN2 of badvaddr
	sll	t1, t1, 3	// << 3 for alignment
	add	t0, t0, t1	// t0 is entry addr of vpt2
	ld	t1, 0(t0)	// t1: actual pte of vpt2
	andi	t2, t1, 0x1		// and PTE_V
	beqz	t2, NOPAGE	// vpt2ent not valid, indicating no page
	li	t0, 0x003FFFFFFFFFFC00
	and	t0, t0, t1	// PTE_TO_PADDR(pte)	((((u_int64_t)(pte)) & 0x003FFFFFFFFFFC00) << 2)
	sll	t0, t0, 2	// t0: paddr of vpt1.

	srl	t1, a1, 21	// a1 is badvaddr
	andi	t1, t1, 0x1ff	// t1 is VPN1 of badvaddr
	sll	t1, t1, 3	// << 3 for alignment
	add	t0, t0, t1	// t0 is entry addr of vpt1
	ld	t1, 0(t0)	// t1: actual pte of vpt1
	andi	t2, t1, 0x1		// and PTE_V
	beqz	t2, NOPAGE	// vpt1ent not valid, indicating no page
	li	t0, 0x003FFFFFFFFFFC00
	and	t0, t0, t1	// PTE_TO_PADDR(pte)	((((u_int64_t)(pte)) & 0x003FFFFFFFFFFC00) << 2)
	sll	t0, t0, 2	// t0: paddr of vpt0.

	srl	t1, a1, 12	// a1 is badvaddr
	andi	t1, t1, 0x1ff	// t1 is VPN0 of badvaddr
	sll	t1, t1, 3	// << 3 for alignment
	add	t0, t0, t1	// t0 is entry addr of vpt0
	ld	t1, 0(t0)	// t1: actual pte of vpt0
	andi	t2, t1, 0x1	// and PTE_V
	beqz	t2, NOPAGE	// vpt0ent not valid, indicating no page
	andi	t2, t1, 0x100	// and PTE_COW
	bnez	t2, COW_Check	// PTE_COW valid, branch to check if on writing
COW_Check_Fail:
	move	a0, t1		// Pass actual PTE to func
	j	perm_error	// Permission error
NOPAGE:
	move	a0, a1		// Set a0 to badvaddr
	move	a1, t4	// Set a1 to paddr of vpt2
	call	pageout
	j	ret_from_exception
COW_Check:
	li	t2, 15		// Err code of writing access fault
	bne	a0, t2, COW_Check_Fail
	move	a0, sp
	call	page_fault_handler
	j	ret_from_exception
END(exc_hand_pgfault)

NESTED(do_refill, 0, sp)
	csrr	t0, satp
	csrr	t1, stvec
/*			//li	k1, '?'
			//sb	k1, 0x90000000
			.extern	mCONTEXT
//this "1" is important
1:			//j 1b
			nop
			lw		k1,mCONTEXT
			and		k1,0xfffff000
			mfc0		k0,CP0_BADVADDR
			srl		k0,20
			and		k0,0xfffffffc
			addu		k0,k1
			
			lw		k1,0(k0)
			nop
			move		t0,k1
			and		t0,0x0200
			beqz		t0,NOPAGE
			nop
			and		k1,0xfffff000
			mfc0		k0,CP0_BADVADDR
			srl		k0,10
			and		k0,0xfffffffc
			and		k0,0x00000fff
			addu		k0,k1

			or		k0,0x80000000
			lw		k1,0(k0)
			nop
			move		t0,k1
			and		t0,0x0200
			beqz		t0,NOPAGE
			nop
			move		k0,k1
			and		k0,0x1
			beqz		k0,NoCOW
			nop
			and		k1,0xfffffbff
NoCOW:
			mtc0		k1,CP0_ENTRYLO0
			nop
			tlbwr

			j		2f
			nop
NOPAGE:
//3: j 3b
nop
			mfc0		a0,CP0_BADVADDR
			lw		a1,mCONTEXT
			nop
				
			sw	 	ra,tlbra
			jal		pageout
			nop
//3: j 3b
nop
			lw		ra,tlbra
			nop

			j	1b
2:			nop

			jr		ra
			nop*/
END(do_refill)



BUILD_HANDLER reserved do_reserved cli
BUILD_HANDLER tlb	do_refill	cli
BUILD_HANDLER mod	page_fault_handler cli
